#include <iostream>
#include <pthread.h>

#include "Sys3D.h"
#include "Quaternion.h"
#include "Rotation.h"

#include "DCFlowControl.h"
#include "DCAComputation.h"

#include "Timer.h"

void *ThreadFunction(void *s)
{        
    DCFlowControl *dca = (DCFlowControl*) s;
    dca->assembly_traversal();
    
    while(1){
//             dca->kinAssembly_traversal();
//             dca->kinSolve_traversal();
        dca->solve_traversal();
//             dca->null_traversal();
    }
    return 0;
}

int main (int argc, const char * argv[])
{        
        const double& q = 1.0;
        const double& u = 0.0;
        
        const int& nbodies = 16;
        const int& nproc = 2;
        const int& niter = 10000;
        int x;
        x = nbodies/2;
       
        RSIM::Sys3D mbs3D(nbodies,nproc);
        mbs3D.initDCANodesForDCAABA();
        mbs3D.setQ();
        
        mbs3D.setU(u);
        
        DCBinaryParallelFlow dca0;        
        DCFlowTerminator dca11,dca12;
                
        dca0.child1 = &dca11;dca0.child2 = &dca12;                

        CommSync sync_0_12;
        
        dca0.child_sync = dca12.parent_sync = &sync_0_12;        

        // declair the calculations
        DCARoot comp0;
        DCAABA comp11,comp12;
        
        // link the computations
        dca0.computation = &comp0;
        dca11.computation = &comp11;
        dca12.computation = &comp12;        
        
        // associate local data with the computations
        comp0.mbs3D = &mbs3D;
        comp0.parent_id = 0;
        comp0.child_id = 1;
        comp0.asm_id = 2;    
        comp0.calc_pinvjac = true;
        
         int id_count = 0;
        comp11.mbs3D = &mbs3D;
        comp11.start_id = 0;    comp11.end_id = x-1;      comp11.asm_id = 0;        
        comp11.kin_startid = 0; comp11.kin_endid = x-1;        
        comp11.calcH2 = true;
        comp11.calc_pinvjac = true;
        mbs3D.ids_[id_count++] = comp11.kin_startid;
                
        comp12.mbs3D = &mbs3D;
        comp12.start_id = x;    comp12.end_id = nbodies-1;     comp12.asm_id = 1;        
        comp12.kin_startid = x; comp12.kin_endid = nbodies-1;        
        comp12.calcH2 = true;
        comp12.calc_pinvjac = true;
        mbs3D.ids_[id_count++] = comp12.kin_startid;
        mbs3D.ids_[id_count++] = comp12.kin_endid+1;
                                
        pthread_attr_t attr;
        pthread_t thread1;

        pthread_attr_init(&attr);                
        pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);        
        
        //set detached so thread resources will be freed immediately upon
        // thread termination (no join needed)
        pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
                
        void *data = (void *) &dca12;
        pthread_create(&thread1, &attr, ThreadFunction, data);        
        
        //clean-up the attributes
        pthread_attr_destroy(&attr);        
        mbs3D.updRotationMatrix();
        mbs3D.updAngularVelocity();
        mbs3D.calcJ();
        dca0.assembly_traversal();                                
        
        RSIM::Timer t;        
        for(int i=0;i<niter;i++)
        {        
                // run the root subsystem
//                 dca0.kinAssembly_traversal();
//                 dca0.kinSolve_traversal();
                dca0.solve_traversal();
//                 dca0.null_traversal();
        }    
        t.stop();
        t.printElapsedTime(0);
//         mbs3D.solveSystemHandleEquationsCase_OpControl();
//         cout<<mbs3D.iLambda_<<endl;
//         cout<<mbs3D.pe_<<endl;
//         mbs3D.collectJac();
//         cout<<mbs3D.Jac_<<endl;
//         cout<<mbs3D.pinvjac_<<endl;
//         t.printElapsedTime(0);                
//         cout<<mbs3D.null_<<endl;
        exit(0);
        return 0;
}
